#include "bsp/bsp.h"
#include "controller.h"
#include "mc_config.h"
#include "utils/logger.h"

void contrl_init(controller_t *ctrl) {
    memset(ctrl, 0, sizeof(controller_t));
	line_ramp_init(&ctrl->curr_target[0], 100);
	line_ramp_init(&ctrl->curr_target[1], 100);
	line_ramp_init(&ctrl->vdq_target[0], 300);
	line_ramp_init(&ctrl->vdq_target[1], 300);
	line_ramp_init(&ctrl->torque_target, 50);
	line_ramp_init(&ctrl->vel_target, 100);

    ctrl->mode_req = ctrl->mode_running = CTRL_MODE_OPEN;
    ctrl->foc.ts = mc_conf()->ts;
	ctrl->foc.pi_curr_id.kp = mc_conf()->c.pid[pid_id].kp;
	ctrl->foc.pi_curr_id.ki = mc_conf()->c.pid[pid_id].ki;
	ctrl->foc.pi_curr_id.ts = mc_conf()->ts;
	ctrl->foc.pi_curr_iq.kp = mc_conf()->c.pid[pid_iq].kp;
	ctrl->foc.pi_curr_iq.ki = mc_conf()->c.pid[pid_iq].ki;
	ctrl->foc.pi_curr_iq.ts = mc_conf()->ts;
	foc_init(&ctrl->foc);
}

void contrl_get_phase_duty(controller_t *ctrl) {
    foc_t *foc = &ctrl->foc;
    foc->phase_angle = ctrl->encoder_angle;
    clark(ctrl->phase_curr[0], ctrl->phase_curr[1], ctrl->phase_curr[2], &ctrl->curr_albt_real.a, &ctrl->curr_albt_real.b);
    fast_sin_cos_f(foc->phase_angle, &foc->phase_sin, &foc->phase_cos);
    park(ctrl->curr_albt_real.a, ctrl->curr_albt_real.b, foc->phase_sin, foc->phase_cos, &foc->curr_dq_real.d, &foc->curr_dq_real.q);
    foc->b_openloop = (ctrl->mode_running == CTRL_MODE_OPEN);
	foc->vbus_vol = ctrl->vbus_vol_sampled;
	foc->m_velocity = ctrl->encoder_velocity;
    foc_update_svpwm(foc);
    u16 ta = foc->duty_norm[0] * mc_conf()->pwm_half_period;
    u16 tb = foc->duty_norm[1] * mc_conf()->pwm_half_period;
    u16 tc = foc->duty_norm[2] * mc_conf()->pwm_half_period;
	pwm_apply_duty(ta, tb, tc);
}

void contrl_update_target(controller_t *ctrl) {
	if (!ctrl->b_start) {
		return;
	}
	float id_set = 0, iq_set = 0;	
	float vd_set = 0, vq_set = 0;
	if (ctrl->mode_running == CTRL_MODE_CURRENT) {
		id_set = line_ramp_step(&ctrl->curr_target[0]);
		iq_set = line_ramp_step(&ctrl->curr_target[1]);
	}else if (ctrl->mode_running == CTRL_MODE_TORQUE) {
		/* the torque is the peek current of the phase, not the real torque */
		float torque = line_ramp_step(&ctrl->torque_target);
		contrl_torque_to_idq(ctrl, torque, &id_set, &iq_set);
	}else if (ctrl->mode_running == CTRL_MODE_VELOCITY){
		float vel_target = line_ramp_step(&ctrl->vel_target);
		float vel_err = vel_target - ctrl->encoder_velocity;
		float torque = pi_run(&ctrl->pi_velocity, vel_err, 0);
		contrl_torque_to_idq(ctrl, torque, &id_set, &iq_set);
	}else {
		vd_set = line_ramp_step(&ctrl->vdq_target[0]);
		vq_set = line_ramp_step(&ctrl->vdq_target[1]);
	}
	uint32_t mask = cpu_enter_critical();
	ctrl->foc.curr_dq_set.d = id_set;
	ctrl->foc.curr_dq_set.q = iq_set;
	ctrl->foc.vol_dq_set.d = vd_set;
	ctrl->foc.vol_dq_set.q = vq_set;
	cpu_exit_critical(mask);
}

bool contrl_enable(controller_t *ctrl, bool start) {
	if (ctrl->b_start == start) {
		return true;
	}
	pi_reset(&ctrl->pi_velocity_limit, 0);
	pi_reset(&ctrl->pi_velocity, 0);

	ctrl->mode_req = CTRL_MODE_OPEN;
	ctrl->mode_running = CTRL_MODE_OPEN;

	foc_init(&ctrl->foc);

	ctrl->b_start = start;
	return true;
}

bool contrl_request_mode(controller_t *ctrl, u8 mode) {
	if (mode > CTRL_MODE_CURRENT) {
		return false;
	}
	ctrl->mode_req = mode;
	return true;
}

u8 contrl_update_mode(controller_t *ctrl) {
	u8 mode_last = ctrl->mode_running;
	if (!ctrl->b_start) {
		ctrl->mode_running = CTRL_MODE_OPEN;
	}else {
		ctrl->mode_running = ctrl->mode_req;
	}
	if (mode_last != ctrl->mode_running) {
		if (ctrl->mode_running == CTRL_MODE_CURRENT) {
			line_ramp_reset(&ctrl->curr_target[0], 0);
			line_ramp_reset(&ctrl->curr_target[1], 0);
		}else if (ctrl->mode_running == CTRL_MODE_TORQUE) {
			line_ramp_reset(&ctrl->torque_target, 0);
		}else if (ctrl->mode_running == CTRL_MODE_VELOCITY){
			line_ramp_reset(&ctrl->vel_target, 0);
		}else {
			line_ramp_reset(&ctrl->vdq_target[0], 0);
			line_ramp_reset(&ctrl->vdq_target[1], 0);
		}
	}
	return ctrl->mode_running;
}